import time

import airsim
from airsim.types import Vector3r
import time
# 连接 AirSim 仿真环境
client = airsim.MultirotorClient()
client.confirmConnection()

# 原始坐标（错误格式：list[list[int]]）
height=2
three = [[
    [2,2 , height],
    [2, -2, height],
    [-2, -2, height],
    [-2, 2, height],
    [2,2 , height],
],[
    [2,2 , -5],
    [2, -2, -5],
    [-2, -2, -5],
    [-2, 2, -5],
    [2,2 , -5],
],
    [
        [-6, 2, -5],
        [-6, -2, -5],
        [-10, -2, -5],
        [-10, 2, -5],
        [-6, 2, -5],
    ],
    [
        [-6, -6, -5],
        [-6, -10, -5],
        [-10, -10, -5],
        [-10, -6, -5],
        [-6, -6, -5],
    ],
[
        [-14, 2, -10],
        [-14, -2, -10],
        [-18, -2, -10],
        [-18, 2, -10],
        [-14, 2, -10],
    ],[
        [-14, -6, -10],
        [-14, -10, -10],
        [-18, -10, -10],
        [-18, -6, -10],
        [-14, -6, -10],
    ],[
        [-22, 2, -10],
        [-22, -2, -10],
        [-26, -2, -10],
        [-26, 2, -10],
        [-22, 2, -10],
    ],[
        [-22, -6, -10],
        [-22, -10, -10],
        [-26, -10, -10],
        [-26, -6, -10],
        [-22, -6, -10],
    ],
[
        [-30, 2, -5],
        [-30, -2, -5],
        [-34, -2, -5],
        [-34, 2, -5],
        [-30, 2, -5],
    ],
    [
        [-30, -6, -5],
        [-30, -10, -5],
        [-34, -10, -5],
        [-34, -6, -5],
        [-30, -6, -5],
    ],
    [
    [-38,2 , height],
    [-38, -2, height],
    [-42, -2, height],
    [-42, 2, height],
    [-38,2 , height],
],[
    [-38,2 , -5],
    [-38, -2, -5],
    [-42, -2, -5],
    [-42, 2, -5],
    [-38,2 , -5],
],
]

# 转换为正确格式：list[Vector3r]
i=1
target_numbers = {4, 6, 8, 10}
try:
    # 错误调用：直接传递 raw_points（会报类型错误）
    # client.simPlotLineStrip(raw_points)

    # 正确调用：转换为 Vector3r 列表
    for init in three:
        vector_points = [Vector3r(x, y, z) for x, y, z in init]
        color_rgba = [0.0, 1.0, 0.0, 1.0]
        if i in target_numbers:
            color_rgba = [1.0, 0.0, 0.0, 1.0]
        client.simPlotLineStrip(
            points=vector_points,
            color_rgba=color_rgba,
            thickness=8,
            duration=3000
        )
        i+=1
    print("线条绘制成功！")
except Exception as e:
    print(f"错误：{e}")
time.sleep(30)
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
client.moveToPositionAsync(0,0,-6,1).join()
client.moveToPositionAsync(-8,0,-6,1).join()
client.moveToPositionAsync(-16,0,-11,1).join()
client.moveToPositionAsync(-24,0,-11,1).join()
client.moveToPositionAsync(-32,0,-5,1).join()
client.moveToPositionAsync(-40,0,-5,1).join()
client.landAsync().join()